#include "tim.h"
tim t;
int light = 1;

int k = 1;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tim");
    ros::NodeHandle nh;
    ros::Rate       looprate(10);
    while ( ros::ok() ) {
        t.run(looprate);
    }
    return 0;
}

void tim::run(ros::Rate& looprate)
{
    // 获取更新之后的值
    ros::param::get("state", state);
    stim_last = stim_cur;
    ros::param::get("stim", stim_cur);
    if ( stim_cur == 1 ) {
        // 延迟10ms
        usleep(10000);
        return;
    }

    if ( stim_cur == 0 && stim_last == 1 && (state == STRIGHT || state == RIGHT || state == LEFT || state == DAOCHEYOUZHAUAN) ) {
        cout << "\033[31m\033[44mbegin to sleep\033[0m" << endl;
        switch ( state ) {
        case STRIGHT:
            ros::param::set("stim", 0);
            for ( int i = 0; i < 600 * k; i++ ) {   // 580
                usleep(10000);
            }
            break;

        case LEFT:
            ros::param::set("stim", 2);
            // 轨迹预测，‘3’
            for ( int i = 0; i < 180 * k; i++ ) {
                ros::param::set("stim", 2);
                usleep(10000);
            }
            cout << "\033[31m\033[44mstim is change to 0\033[0m" << endl;

            ros::param::set("stim", 0);
            // 轨迹预测，‘1’
            for ( int i = 0; i < 240; i++ ) {
                ros::param::set("stim", 0);
                usleep(10000);
            }
            ros::param::set("state", STRIGHT);
            break;

        case RIGHT:
            ros::param::set("stim", 0);
            for ( int i = 0; i < 350; i++ ) {   // 350
                ros::param::set("stim", 0);
                usleep(10000);
            }
            break;
            case DAOCHEYOUZHAUAN:
                ros::param::set("stim", 2);
                for ( int i = 0; i < 50; i++ ) {   // 350
                    ros::param::set("stim", 2);
                    usleep(10000);
                }
                ros::param::set("stim", 0);
                for ( int i = 0; i < 300; i++ ) {   // 350
                    ros::param::set("stim", 0);
                    usleep(10000);
                }
                break;

        }
        ros::param::set("stim", 1);
        ros::param::set("state", 0);
    }
}

tim::tim()
{
    stim_last = 1;
    stim_cur  = 1;
    state     = 7;
}
